#include "PhysicsObject.h"
#include "OpenGL.h"
#include <math.h>
#include <float.h>
#include <iostream>
using namespace std;
//****Prototypes ****
float positionTest(float t,PhysicsObject* th, PhysicsObject* o);	//Equation of motion, calculates the distance between two Physics objects' positions at time t
float brentSolver(float (*f)(float, PhysicsObject*, PhysicsObject*), float a, float b, float& xmin, PhysicsObject* th, PhysicsObject* o);
//****PhysicsState implementation ****
PhysicsState::_PhysicsState(){ }
PhysicsState::_PhysicsState(const PhysicsState& o){
	position = o.position;
	momentum = o.momentum;
	orientation = o.orientation;
	angularMomentum = o.angularMomentum;
}
_PhysicsState& PhysicsState::operator=(const PhysicsState& o){
	if(&o!=this){
		position = o.position;
		momentum = o.momentum;
		orientation = o.orientation;
		angularMomentum = o.angularMomentum;
	}
	return *this;
}
bool PhysicsState::operator==(const PhysicsState& o) const{
	return position==o.position && momentum==o.momentum && orientation==o.orientation && angularMomentum==o.angularMomentum;
}
bool PhysicsState::operator!=(const PhysicsState& o) const{
	return !(*this==o);
}
//**** OBoundingBox implmentation ****
OBoundingBox::_OBoundingBox() { }
OBoundingBox::_OBoundingBox(const OBoundingBox& o){
	position = o.position;
	extents = o.extents;
	orientation = o.orientation;
}
OBoundingBox& OBoundingBox::operator=(const OBoundingBox& o){
	if(&o != this){
		position = o.position;
		extents = o.extents;
		orientation = o.orientation;
	}
	return *this;
}
bool OBoundingBox::operator==(const OBoundingBox& o) const{
	return position==o.position && extents==o.extents && orientation==o.orientation;
}
bool OBoundingBox::operator!=(const OBoundingBox& o) const{
	return !(*this==o);
}
const PhysicsVector OBoundingBox::xAxis(){
	PhysicsQuaternion p = orientation.crossProduct(PhysicsQuaternion(0,extents[0],0,0).crossProduct(orientation.inverse()));
	return PhysicsVector(p[1],p[2],p[3]);
}
const PhysicsVector OBoundingBox::yAxis(){
	PhysicsQuaternion p = orientation.crossProduct(PhysicsQuaternion(0,0,extents[1],0).crossProduct(orientation.inverse()));
	return PhysicsVector(p[1],p[2],p[3]);
}
const PhysicsVector OBoundingBox::zAxis(){
	PhysicsQuaternion p = orientation.crossProduct(PhysicsQuaternion(0,0,0,extents[2]).crossProduct(orientation.inverse()));
	return PhysicsVector(p[1],p[2],p[3]);
}
//**** PhysicsObject implementation ****
PhysicsObject::PhysicsObject(){ }
void PhysicsObject::update(float t, float dt){
	previous = current;
	current = intergrate(t, dt, true);	//Updating the physical state requires collision detection.
}
PhysicsState PhysicsObject::evalDerivative(const PhysicsState& init, float t, float dt, const PhysicsState& d, bool collision){
	PhysicsState s, ret;
	s.position = init.position + d.position * dt;
	s.momentum = init.momentum + d.momentum * dt;
	s.orientation = init.orientation + d.orientation * dt;
	s.orientation.normalize();
	s.angularMomentum = init.angularMomentum + d.angularMomentum * dt;
	
	ret.position = s.momentum * inverseMass;
	PhysicsVector angularVelocity = (s.angularMomentum / inertia());
	ret.orientation = 0.5f * PhysicsQuaternion(0, angularVelocity[0], angularVelocity[1], angularVelocity[2]).crossProduct(s.orientation);
	ret.momentum = Forces(s, t+dt, collision);
	ret.angularMomentum = PhysicsVector(0.57735f,0.57735f,0.57735f) - angularVelocity*0.1f;
	return ret;
}
PhysicsState PhysicsObject::intergrate(float t, float dt, bool collision){
	PhysicsState a, b, c, d, future=previous;
	a = evalDerivative(future, t, 0, a, collision);
	b = evalDerivative(future, t, dt*0.5f, a, collision);
	c = evalDerivative(future, t, dt*0.5f, b, collision);
	d = evalDerivative(future, t, dt, c, collision);
	future.position += 1.0f/6.0f * dt * (a.position + 2.0f * (b.position + c.position) + d.position);
	future.momentum += 1.0f/6.0f * dt * (a.momentum + 2.0f * (b.momentum + c.momentum) + d.momentum);
	future.orientation += 1.0f/6.0f * dt * (a.orientation + 2.0f * (b.orientation + c.orientation) + d.orientation);
	future.angularMomentum += 1.0f/6.0f * dt * (a.angularMomentum + 2.0f * (b.angularMomentum + c.angularMomentum) + d.angularMomentum);
	future.orientation.normalize();
	return future;
}
PhysicsVector PhysicsObject::Forces(const PhysicsState& s, float t, bool collision){
	PhysicsVector ret;
	if(gravity) ret += PhysicsVector(0,-9.8f*mass,0);//Add in gravity
	ret += force;
	if(collision) ;
	//Collision detection & response goes here.
	return ret;
}
void PhysicsObject::render(){
	glPushMatrix();		//Save last matrix
	glLoadIdentity();	//Set to valid viewport state
	glEnable(GL_LIGHTING);
	glTranslatef(current.position[0],current.position[1],current.position[2]);	//Go to object position.
	PhysicsVector spinAxis;
	float angle;
	current.orientation.angleAxis(angle,spinAxis);	//Get spin axis
	glRotatef(angle*57.2957795130f,spinAxis[0],spinAxis[1],spinAxis[2]);	//Rotate, converting to degrees
	GLfloat color[] = { 1.0f, 0.0f, 0.0f, 1.0f };
	glMaterialfv(GL_FRONT, GL_AMBIENT, color);
	glMaterialfv(GL_FRONT, GL_DIFFUSE, color);
	renderInternal();	//render the object
	glDisable(GL_LIGHTING);
	glPopMatrix();	//Return from whence we came.
}
PhysicsVector PhysicsObject::collision(PhysicsObject* o, float dt, float& t){
	PhysicsVector normal;
	if(boundingSphereTest(o, dt, normal)){
		t += dt;
		return collisionResponse(o,normal,t);
	}
	return PhysicsVector(0,0,0);
}
bool PhysicsObject::boundingSphereTest(PhysicsObject* o, float& dt, PhysicsVector& normal){
	//Relative Position
	PhysicsVector Pr(previous.position - o->previous.position);
	const float r = getBoundingSphere() + o->getBoundingSphere();
	const float r2 = r * r;	//Square radius
	const float prpr = Pr * Pr;
	if(prpr <= r2){	//Already collided at normalized t=0
		dt=0;
		const float prn = sqrt(prpr);
		normal = Pr * (r - prn) / prn;
		return true;
	}
	//Relative velocity
        PhysicsVector Vr(previous.momentum*inverseMass - o->previous.momentum*o->inverseMass);
	if(Pr*Vr>0)	//Moving away from each other
		return false;
	float t = 0;
	float p = brentSolver(positionTest,0.0f,dt,t,this,o);
	cout<<"dt="<<dt<<endl;
	cout<<"f("<<t<<")="<<p<<" vs. "<<r2<<endl;
	if(p<=r2) cout<<"p<=r2"<<endl;
	if(p<=r2 && (t>FLT_EPSILON && t<dt)){ 	//Spheres collide!
		cout<<"here"<<endl;
		dt=t;
		return true;
	}
	//Spheres breeze by.
	return false;
}
float positionTest(float t,PhysicsObject* th, PhysicsObject* o){
	PhysicsState o1 = th->intergrate(0,t,false);
	PhysicsState o2 = o->intergrate(0,t,false);
	return (o1.position - o2.position).squareNorm();
}
#define CGOLD (0.3819660f)
#define EPSILON FLT_EPSILON
#define STEPS (100000.0f)
float brentSolver(float (*f)(float, PhysicsObject*, PhysicsObject*), float a, float b, float& xmin, PhysicsObject* t, PhysicsObject* o){
cout<<"---------"<<endl;
	float bx,r,q,p,v,w,x,d=0.0f,u,fu;
	bx = 0.5f*(a+b);
	if(a>b){ float t=a; a=b; b=t; }	//Swap so lowest is a
	w = v = x = bx;
	float e=0.0f,	fx=f(x,t,o), fv=fx, fw=fx;
	for(int i=1;i<STEPS;i++){
		//Let Xm be the average of the interval
		float xm=0.5f*(a+b);
		if(fabs(x-xm)<=EPSILON*2-0.5*(b-a))
			break;
		if(fabs(e)>EPSILON){
			r=(x-w)*(fx-fv); q=(x-v)*(fx-fw); p=(x-v)*q-(x-w)*r; q=2*(q-r);
			if(q>0) p=-p;
			q=fabs(q);
			float etemp = e;
			e=d;
			if(fabs(p)>=fabs(0.5*q*etemp) && p<=q*(a-x) && p>=q*(b-x)){
				d=p/q;
				u=x+d;
				if(u-a<EPSILON*2||b-u<EPSILON*2)
					d = (xm-x>0 ? EPSILON : -EPSILON);
			}
			else{
				e = (x>=xm ? a-x : b-x);
				d = CGOLD*e;
			}
		}
		else{
			e = (x>=xm ? a-x : b-x);
			d = CGOLD*e;
		}
		u=(fabs(d)>=EPSILON ? x+d : x+(d>0?EPSILON:-EPSILON));
		fu=f(u,t,o);
		if(fu<=fx){
			if(u>=x) a=x;
			else b=x;
			v=w; fv=fw; w=x; fw=fx; x=u; fx=fu;
		}
		else{
			if(u<x) a=u;
			else b=u;
			if(fu<=fw || w==x){
				v=w; fv=fw; w=u; fw=fu;
			}
			else if(fu<=fv || v==x || v==2){ v=u; fv=fu; }
		}
	}
	xmin = x;
	return fx;	
}
#define COLLISION_RESPONSE_CONSTANT 100
PhysicsVector PhysicsObject::collisionResponse(PhysicsObject* o, PhysicsVector n, float t){
	//Assuming elastic collision.
	return n*(COLLISION_RESPONSE_CONSTANT);
}
void PhysicsObject::setPosition(PhysicsVector x){
	current.position = x;
	previous.position = x;
}
//PhysicsEngine implementation
std::list<PhysicsObject*> _Objects;
void PhysicsEngine::addObject(PhysicsObject& obj){
	_Objects.push_back(&obj);
}
std::list<PhysicsObject*>& PhysicsEngine::getObjects(){
	return _Objects;
}
void PhysicsObject::setMass(float _mass){
	mass = _mass;
	inverseMass = 1.0/mass;
}
float PhysicsObject::getMass(){
	return mass;
}
